#ifndef __SKY_SEACHER_H__
#define __SKY_SEACHER_H__

#include <vector>

#include "astro_coordinates/ephemeris.h"
#include "motor_controller.h"
#include "sky_seger.h"

#include <opencv2/opencv.hpp>

enum CamId {
    CAM_WIDE = 0,// 广角
    CAM_TELE// 长焦
};

class SkySeacher {
private:
    SkySeger sky_seger_;

    StepMotorUser& motor_yaw_;
    StepMotorUser& motor_pitch_;

    const int motor_resolution_level_ = STEP_MOTOR_MINISTEP_8;
    const int motor_speed_ramping_ = STEP_MOTOR_RAMP_PULSE_1000;
    const double motor_speed_ = 32.0;

    const double motor_pitch_reset_offset_angle = 8.0;
    const double motor_pitch_reset_coord_to_mechanical_coord = 90 - motor_pitch_reset_offset_angle;

    // 广角参数
    CamWide cam_wide_;
    const double exposure_ = 1.0;
    const int gain_ = 164;

    const double cam_wide_width_fov = 38.9;
    const double cam_wide_height_fov = 22.4;

    const double cam_tele_width_fov = 3.18;
    const double cam_tele_height_fov = 1.79;

    // 像素坐标与三维坐标互转
    const double cam_wide_f_ = 6780.0;// 理论焦距6.78mm，6780um
    const double cam_wide_pix_size_ = 2.8;// 广角像素大小2.8um 
    const double cam_wide_f_pix_ = cam_wide_f_ / cam_wide_pix_size_;// 广角像素焦距
    
    const double cam_wide_resolution_x_ = 1920;// 与pix_size对应的分辨率
    const double cam_wide_resolution_y_ = 1080;
    const double cam_wide_center_pix_x_ = cam_wide_resolution_x_ / 2.0;
    const double cam_wide_center_pix_y_ = cam_wide_resolution_y_ / 2.0;

    // 像平面在三维空间的x轴上
    const cv::Mat cam_wide_matrix_pixel_to_camera_coord_ = (cv::Mat_<double>(3, 3) << 1.0, 0, 0, 
                                                                                    cam_wide_center_pix_x_, cam_wide_f_pix_, 0, 
                                                                                    cam_wide_center_pix_y_, 0, -cam_wide_f_pix_);

    const double cam_tele_f_ = 100000.0;// 理论焦距100mm，10^5um
    const double cam_tele_pix_size_ = 1.45;// 长焦像素大小1.45um 
    const double cam_tele_f_pix_ = cam_tele_f_ / cam_tele_pix_size_;// 长焦像素焦距

    const double cam_tele_resolution_x_ = 3840;// 与pix_size对应的分辨率
    const double cam_tele_resolution_y_ = 2160;
    const double cam_tele_center_pix_x_ = cam_tele_resolution_x_ / 2.0;
    const double cam_tele_center_pix_y_ = cam_tele_resolution_y_ / 2.0;

    // 像平面在三维空间的x轴上
    const cv::Mat cam_tele_matrix_pixel_to_camera_coord_ = (cv::Mat_<double>(3, 3) << 1.0, 0, 0, 
                                                                                    cam_tele_center_pix_x_, cam_tele_f_pix_, 0, 
                                                                                    cam_tele_center_pix_y_, 0, -cam_tele_f_pix_);

    // 校准参数
    double calibration_rotate_angle_ = 5.0;

private:
    int setCamWide();

    bool isTeleViewGood(cv::Mat& img_sky_mask, const std::vector<cv::Point>& tele_view);
    int testAllTeleViewInWideView(cv::Mat& img_sky_mask, const HorizontalCoordinates& center_mechanical_coord_wide_view, std::vector<HorizontalCoordinates>& calibration_path);
    int testSameAltTeleViewInWideView(cv::Mat& img_sky_mask, const HorizontalCoordinates& center_mechanical_coord_wide_view, const HorizontalCoordinates& center_mechanical_coord_tele_view, std::vector<HorizontalCoordinates>& calibration_path);
    int getTeleViewInWideView(const HorizontalCoordinates& center_mechanical_coord_wide_view, const HorizontalCoordinates& center_mechanical_coord_tele_view, std::vector<cv::Point>& points);
    bool isPointOutOfWideView(cv::Point& point);
    int pixelCoordToMechanicalCoord(int cam_id, const cv::Point& pixel_coord, const HorizontalCoordinates& center_mechanical_coord, HorizontalCoordinates& mechanical_coord);
    int mechanicalCoordToPixelCoord(int cam_id, const HorizontalCoordinates& mechanical_coord, const HorizontalCoordinates& center_mechanical_coord, cv::Point& pixel_coord);

public:
    SkySeacher() : motor_yaw_(MotorController::motorYaw()), motor_pitch_(MotorController::motorPitch()) {}
    int start(std::vector<HorizontalCoordinates>& calibration_path);
    // TODO: 记得让顶层传参
    void setCalibrationRotateAngle(double calibration_rotate_angle) { calibration_rotate_angle_ = calibration_rotate_angle; }

    void test();
};

#endif